#include <iostream>
#include <vector>
#include <cmath>
#include "location.h"
#include "item.h"
#include "mapitem.h"
#include "astarque.h"
#include "node.h"
#include "astar.h"
using namespace std;

Astar::Astar(Location*& location, int fromx, int fromy, int tox, int toy){
	to_ = new Node(tox,toy);
	from_ = new Node(fromx,fromy);
	loc_ = location;

	beginSearch();
}

Astar::~Astar(){
	delete to_;
	delete from_;
}

vector<Node*> Astar::getWay(){
	return way_;
}

void Astar::beginSearch(){
	// Create a queNode for the startnode
	Astarque* startNode = new Astarque(from_->getx(), from_->gety(), 0, calcEstimated(from_->getx(), from_->gety()), vector<Node*>());
	
	//Add the start node to the que
	que_.push_back(startNode);

	bool found = false;
	int expandednodes = 0;
	while(que_.size() > 0 && found == false)
	{
		// Check if we've found the shortest route to the end node
		if(que_[0]->getx() == to_->getx() && que_[0]->gety() == to_->gety())
		{
			found = true;
			way_ = que_[0]->getWay();
		}
		else
		{
			expandednodes++;
			expandNode(que_[0]);
		}
	}
	
}

void Astar::expandNode(Astarque* queNode){
	int x;
	int y;
	int distFromStart;
	double totalDist;
	vector<Node*> way;

	// Add this node to the hasChecked node list and remove from que
	Node* checked = new Node(queNode->getx(), queNode->gety());
	hasChecked_.push_back(checked);
	checked = 0;
	delete checked;
	
	que_.erase(que_.begin());
		
	if((queNode->gety()%2) == 0)
	{
		// -1 +1
		if((queNode->getx() - 1) >= 0 && (queNode->gety() + 1) <= 46 && loc_->map_[(queNode->getx() - 1)][(queNode->gety() + 1)]->getWalkable()){
			x = (queNode->getx() - 1);
			y = (queNode->gety() + 1) ;
			Node* wayNode = new Node(x,y);
			
			if(!hasBeenChecked(wayNode))
			{
				distFromStart = (queNode->distanceFromStart() + 1);
				totalDist = (distFromStart + calcEstimated(x, y));
				way = queNode->getWay();
				way.push_back(wayNode);
				
				Astarque* neighbour = new Astarque(x, y, distFromStart, totalDist, way);
				addNeighbours(neighbour);
			}
		}

		// -1 -1
		if((queNode->getx() - 1) >= 0 && (queNode->gety() - 1) >= 0 && loc_->map_[(queNode->getx() - 1)][(queNode->gety() - 1)]->getWalkable()){
			x = (queNode->getx() - 1);
			y = (queNode->gety() - 1) ;
			Node* wayNode = new Node(x,y);
			
			if(!hasBeenChecked(wayNode))
			{
				distFromStart = (queNode->distanceFromStart() + 1);
				totalDist = (distFromStart + calcEstimated(x, y));
				way = queNode->getWay();
				way.push_back(wayNode);
				
				Astarque* neighbour = new Astarque(x, y, distFromStart, totalDist, way);
				addNeighbours(neighbour);
			}
		}

		// 0 -1
		if((queNode->gety() - 1) >= 0 && loc_->map_[queNode->getx()][(queNode->gety() - 1)]->getWalkable()){
			x = queNode->getx();
			y = (queNode->gety() - 1) ;
			Node* wayNode = new Node(x,y);
			
			if(!hasBeenChecked(wayNode))
			{
				distFromStart = (queNode->distanceFromStart() + 1);
				totalDist = (distFromStart + calcEstimated(x, y));
				way = queNode->getWay();
				way.push_back(wayNode);
				
				Astarque* neighbour = new Astarque(x, y, distFromStart, totalDist, way);
				addNeighbours(neighbour);
			}
		}
		
		
		// 0 +1
		if((queNode->gety() + 1) <= 46 && loc_->map_[queNode->getx()][(queNode->gety() + 1)]->getWalkable()){
			x = queNode->getx();
			y = (queNode->gety() + 1) ;
			Node* wayNode = new Node(x,y);
			
			if(!hasBeenChecked(wayNode))
			{
				distFromStart = (queNode->distanceFromStart() + 1);
				totalDist = (distFromStart + calcEstimated(x, y));
				way = queNode->getWay();
				way.push_back(wayNode);
				
				Astarque* neighbour = new Astarque(x, y, distFromStart, totalDist, way);
				addNeighbours(neighbour);
			}
		}
		
		
	}
	else
	{

		// +1 -1
		if((queNode->getx() + 1) <= 16 && (queNode->gety() - 1) >= 0 && loc_->map_[(queNode->getx() + 1)][(queNode->gety() - 1)]->getWalkable()){
			x = (queNode->getx() + 1);
			y = (queNode->gety() - 1) ;
			Node* wayNode = new Node(x,y);
			
			if(!hasBeenChecked(wayNode))
			{
				distFromStart = (queNode->distanceFromStart() + 1);
				totalDist = (distFromStart + calcEstimated(x, y));
				way = queNode->getWay();
				way.push_back(wayNode);
				
				Astarque* neighbour = new Astarque(x, y, distFromStart, totalDist, way);
				addNeighbours(neighbour);
			}
		}

		// 0 -1
		if((queNode->gety() - 1) >= 0 && loc_->map_[(queNode->getx())][(queNode->gety() - 1)]->getWalkable()){
			x = (queNode->getx());
			y = (queNode->gety() - 1) ;
			Node* wayNode = new Node(x,y);
			
			if(!hasBeenChecked(wayNode))
			{
				distFromStart = (queNode->distanceFromStart() + 1);
				totalDist = (distFromStart + calcEstimated(x, y));
				way = queNode->getWay();
				way.push_back(wayNode);
				
				Astarque* neighbour = new Astarque(x, y, distFromStart, totalDist, way);
				addNeighbours(neighbour);
			}
		}

		// +1 +1
		if((queNode->getx() + 1) <= 16 && (queNode->gety() + 1) <= 46 && loc_->map_[(queNode->getx() + 1)][(queNode->gety() + 1)]->getWalkable()){
			x = (queNode->getx() + 1);
			y = (queNode->gety() + 1);
			Node* wayNode = new Node(x,y);
			
			if(!hasBeenChecked(wayNode))
			{
				distFromStart = (queNode->distanceFromStart() + 1);
				totalDist = (distFromStart + calcEstimated(x, y));
				way = queNode->getWay();
				way.push_back(wayNode);
				
				Astarque* neighbour = new Astarque(x, y, distFromStart, totalDist, way);
				addNeighbours(neighbour);
			}
		}

		// 0 +1
		if((queNode->gety() + 1) <= 46 && loc_->map_[(queNode->getx())][(queNode->gety() + 1)]->getWalkable()){
			x = (queNode->getx());
			y = (queNode->gety() + 1) ;
			Node* wayNode = new Node(x,y);
			
			if(!hasBeenChecked(wayNode))
			{
				distFromStart = (queNode->distanceFromStart() + 1);
				totalDist = (distFromStart + calcEstimated(x, y));
				way = queNode->getWay();
				way.push_back(wayNode);
				
				Astarque* neighbour = new Astarque(x, y, distFromStart, totalDist, way);
				addNeighbours(neighbour);
			}
		}				

  
	}
	
}

void Astar::addNeighbours(Astarque* queNode){
	
	bool found = false;
	
	for(int i = 0; i < que_.size(); i++)
	{
		if(que_[i]->getx() == queNode->getx() && que_[i]->gety() == queNode->gety())
		{
			found = true;
			
			if(que_[i]->distanceFromStart() > queNode->distanceFromStart())
			{
				// Replace the node in que_ with the new better queNode
				Astarque* garbage = new Astarque();
				garbage = que_[i];
				que_[i] = 0;
				delete garbage;
				
				que_[i] = queNode;
			    
			}
		}
	}

	// Add the node if it didn't already exist on the que
	if(found == false)
	{
		que_.push_back(queNode);
	}

	// Sort the que since new nodes might be added
	sortQue();
}

bool Astar::hasBeenChecked(Node* node){
	bool found = false;
	for(int i = 0; i < hasChecked_.size(); i++)
	{
		if(node->getx() == hasChecked_[i]->getx() && node->gety() == hasChecked_[i]->gety())
		{
			found = true;
		}
	}
	
	return found;
	
}

void Astar::sortQue(){
	Astarque* queObj = new Astarque();
	vector<Astarque*> sortedQue;
	int remove = 0;
	
	// Start compare with the first object in the que
	if(que_.size() > 0)
	{
		queObj = que_[0];
	}

	// Sort all objekts in the que
	while(que_.size() > 0)
	{
		// Find the ohject in the que_ with the smallest distance from start
		for(int i = 0; i < que_.size(); i++)
		{
			if(que_[i]->estimatedTotalDistance() < queObj->estimatedTotalDistance())
			{
				queObj = 0;
				queObj = que_[i];
				remove = i;
			}
			
		}
		
		// Push the object to the new que sorted by estimatedTotalDistance
		// and remove it from the original que
		sortedQue.push_back(queObj);
		queObj = 0;
		que_[remove] = 0;
		que_.erase(que_.begin()+remove);
		
		// Start comparing the first object in the vector again
		if(que_.size() > 0)
		{
			queObj = que_[0];
			remove = 0;
		}
		
		
	}

	que_ = sortedQue;
}

// Calculate the estimated disance from the node to the end
double Astar::calcEstimated(int x, int y){
	int xdiff = abs(to_->getx() - x);
	int ydiff = abs(to_->gety() - y);
	
	return (xdiff + ydiff);
 	
}
